#!coding:utf-8
import serial
import time
import ctypes
import math

# Message values
BYTE_ACK = 0x04
BYTE_NACK = 0x02
BYTE_SYNC = 0x00
CMD_SELECT = 0x43
CMD_TYPE = 0x40
CMD_MODES = 0x49
CMD_SPEED = 0x52
CMD_MASK = 0xC0
CMD_INFO = 0x80
CMD_LLL_MASK = 0x38
CMD_LLL_SHIFT = 3
CMD_MMM_MASK = 0x07
CMD_DATA = 0xc0
CMD_WRITE = 0x44

MSG_SYSTEM = 0b00
MSG_CMD  = 0b01 << 6
MSG_INFO = 0b10 << 6
MSG_DATA = 0b11 << 6
MSG_MASK = MSG_DATA

TYPE_COLOR = 29

# Values for status
RESET = 0
STARTED = 1
DATA_MODE = 2

# Maximum number of modes supported
MAX_MODES = 10

# The maximum number of data items in a sample
MAX_DATA_ITEMS = 10

# The time between heartbeats in milliseconds
HEART_BEAT = 100

def chk_sum(data):
    # data[-1] = 0
    # print(data)
    check = (~(sum(data[2:]) & 0xFF)) & 0xFF
    return check

def to_int32(value):
    if len(value) >= 4:
        cint32 = ctypes.c_int32((value[3] << 24) | (
            value[2] << 16) | (value[1] << 8) | value[0])
        return cint32.value
    else:
        return 0
    
def to_float32(value):
    if len(value) >= 4:
        cfloat = ctypes.c_float((value[3] << 24) | (
            value[2] << 16) | (value[1] << 8) | value[0])
        return cfloat.value
    else:
        return 0

def to_value(data):
    l = len(data)
    if l == 1:
        return data[0]
    elif l == 2:
        return data[0] | (data[1] << 8)
    elif l == 4:
        n = l//4
        arr = []
        for i in range(n):
            arr.append(data[i*4:i*4+4])
        # print(arr)
        return to_int32(data[:4])
    elif l >= 4 and l//2 == 16:
        n = l//2
        arr = []
        for i in range(0,n):
            arr.append(data[i*2]+(data[i*2+1]<<8))
        return arr
        # print(arr)
        # r = (arr[6]*255//10100) & 0xff
        # g = (arr[3]*255//10100) & 0xff
        # b = (arr[1]*255//10100) & 0xff
        # w = (arr[8]*255//10100) & 0xff
        # return to_int32([r,g,b,w])
    else:
        print(data)
        return 0

class EV3Sensor(object):
    """
    serial client
    """

    def __init__(self, port, callback=None):
        self.reset()
        self.device = port
        self.callback = callback
        self.active = True

    def reset(self):
        self.sensor_id = 0
        self.sensor_mode = 0
        self.speed = 0
        self.init = False
        self.last_nack = time.time()
        # self.last_nack = 0
        self.new_data = False

    def read_data_loop(self):
        while True:
            if self.active:
                self.read_data_loop2()
            else:
                time.sleep(1)

    def read_data_loop2(self):
        self.port = serial.Serial(port=self.device, baudrate=2400, bytesize=8, parity=serial.PARITY_NONE,
                                  stopbits=serial.STOPBITS_ONE, timeout=0.1)
        self.port.flushInput()
        while self.active:
            msg = []
            try:
                msg = self.read_num(1)
            except Exception as e:
                print(e)
            if len(msg) > 0:
                if self.new_data == False and time.time() - self.last_nack > 5:
                    print('called close')
                    self.port.close()
                    time.sleep(0.5)
                    self.port = serial.Serial(port=self.device, baudrate=2400, bytesize=8, parity=serial.PARITY_NONE,
                                            stopbits=serial.STOPBITS_ONE, timeout=0.1)
                    self.port.flushInput()
                    self.last_nack = time.time()
                msg = msg[0]
                if msg != BYTE_SYNC:
                    # print(bin(msg))
                    pass

                if msg & MSG_MASK == MSG_SYSTEM:
                    if msg == BYTE_SYNC:
                        pass
                        # print('BYTE_SYNC')
                        # self.send_hex(BYTE_SYNC)
                        # continue
                    else:
                        print('MSG_SYSTEM')
                    if msg == BYTE_ACK:
                        print('BYTE_ACK')
                        if self.sensor_id > 0 and self.speed > 0:
                            # pass
                            self.send_hex([BYTE_ACK])
                            self.init = True
                            time.sleep(0.2)
                            self.port.baudrate = self.speed
                            self.new_data = False
                            self.last_nack = time.time()
                            # time.sleep(0.1)
                            # self.port.close()
                            # self.port = serial.Serial(port=self.device, baudrate=1152200, bytesize=8, parity=serial.PARITY_NONE,
                            #                         stopbits=serial.STOPBITS_ONE, timeout=0.05)
                            # self.port.flushInput()
                            # self.new_data = time.time()
                            # self.set_mode(0)
                            # self.send_hex(BYTE_NACK)
                    if msg == BYTE_NACK:
                        print('BYTE_NACK')
                elif msg & MSG_MASK == MSG_CMD:
                    print('MSG_CMD')
                    if msg == CMD_TYPE:
                        print('CMD_TYPE')
                        # 回报传感器类型
                        self.sensor_id = self.read_num(1)
                        if len(self.sensor_id) > 0 and self.sensor_id[0] > 0:
                            self.sensor_id = self.sensor_id[0]
                        else:
                            self.sensor_id = 0
                        print(self.sensor_id)
                        if self.callback is not None:
                            # self.callback(to_value(payload))
                            self.callback(0, [self.sensor_id])
                    if msg == CMD_MODES:
                        print('CMD_MODES')
                        sensor_modes = self.read_num(1)
                        sensor_modes = sensor_modes[0]
                        view_modes = self.read_num(1)
                        view_modes = view_modes[0]
                        print(sensor_modes, view_modes)
                    if msg == CMD_SPEED:
                        print('CMD_SPEED')
                        sensor_speed = self.read_num(4)
                        self.speed = to_int32(sensor_speed)
                        print(self.speed)
                    if msg == CMD_SELECT:
                        print('CMD_SELECT')
                        mode = self.read_num(1)
                        print(mode)
                    checksum = self.read_num(1)
                elif msg & MSG_MASK == MSG_INFO:
                    print('MSG_INFO')
                    info = self.read_num(1)
                    if len(info) == 1:
                        info = info[0]
                    else:
                        continue
                    length = int(math.pow(2, ((msg & CMD_LLL_MASK) >> CMD_LLL_SHIFT)))
                    sensor_mode = msg & CMD_MMM_MASK
                    payload = self.read_num(length)
                    if info == 0:
                        print('MODE NAME', sensor_mode, str(payload))
                    if info == 4:
                        print('SYMBOL NAME', sensor_mode, str(payload))
                    if info == CMD_INFO:
                        S = payload[0]
                        T = payload[1]
                        F = payload[2]
                        D = payload[3]
                        print('FORMAT',S,T,F,D)
                    if info >= 1 and info <=3:
                        low = to_float32(payload[:4])
                        high = to_float32(payload[4:])
                        print('RANGE',low,high)
                    checksum = self.read_num(1)
                if msg & MSG_MASK == MSG_DATA:
                    # 回报传感器数据
                    # print('MSG_DATA', int(time.time() - self.last_nack ))
                    length = int(math.pow(2, ((msg & CMD_LLL_MASK) >> CMD_LLL_SHIFT)))
                    # 传感器模式
                    sensor_mode = msg & CMD_MMM_MASK
                    payload = self.read_hex(length)
                    # print(payload)
                    if self.callback is not None and len(payload) > 0:
                        # self.callback(to_value(payload))
                        self.callback(1, [sensor_mode])
                        self.callback(2, payload)
                    self.new_data = True
                    checksum = self.read_num(1)
            if self.init and self.new_data and time.time() - self.last_nack > 0.1:
                self.send_hex([BYTE_NACK])
                self.last_nack = time.time()
                self.new_data = False
            if self.init and time.time() - self.last_nack > 1.2:
                print('called reset')
                self.init = False
                self.new_data = False
                self.sensor_id = 0
                self.port.baudrate = 2400
                if self.callback is not None:
                    # self.callback(to_value(payload))
                    self.callback(0, [self.sensor_id])
                time.sleep(0.2)
                # 回报传感器断开
                # print('传感器已断开')
                # self.port.close()
                # self.port = serial.Serial(port=self.device, baudrate=2400, bytesize=8, parity=serial.PARITY_NONE,
                #                         stopbits=serial.STOPBITS_ONE, timeout=0.05)
                # self.port.flushInput()
        self.port.close()
        print('read data loop2 ', self.device, 'closed')
        
    def send_cmd(self, cmd):
        self.port.write(cmd.encode('utf-8'))

    def read_cmd(self):
        response = self.port.readline()
        return response.decode('utf-8')

    def read_num(self, num):
        response = []
        try:
            response = self.port.read(num)
        except Exception as e:
            print(e)
        if len(response) > 0:
            # print('data receive: ',list(response))
            pass
        return response

    def send_hex(self, cmd):
        # print(cmd)
        self.port.write(cmd)
        # time.sleep(0.00001)
        # self.port.flushInput()

    def read_hex(self, n=0):
        array = []
        try:
            if(n > 0):
                response = self.port.read(n)
            else:
                response = self.port.readline()
            for i in response:
                array.append(i)
        except Exception as e:
            print(e)
        return array

    def set_mode(self, mode = 0):
        checksum = 0xff ^ CMD_SELECT
        self.port.write([CMD_SELECT])
        self.port.write([mode])
        checksum ^= mode
        self.port.write([checksum])

    def set_value(self, value = 0):
        # length=4 2^(0b010)
        header = 70
        checksum = 0xff ^ header
        r = value & 0xff
        g = (value >> 8) & 0xff
        b = (value >> 16) & 0xff
        a = (value >> 24) & 0xff
        self.port.write([header, r, g, b, a])
        checksum ^= r
        checksum ^= g
        checksum ^= b
        checksum ^= a
        self.port.write([checksum])

if __name__ == '__main__':
    import serial.tools.list_ports
    serial_ports = [i[0] for i in serial.tools.list_ports.comports()]
    print(serial_ports)
    # sensor = EV3Sensor('/dev/ttyAMA0') # S5
    # sensor = EV3Sensor('/dev/ttyAMA2') # S4
    sensor = EV3Sensor('/dev/ttyAMA4') # S3
    sensor.read_data_loop()